﻿#ifndef PATHELEMENT_H
#define PATHELEMENT_H

#include "global.h"
#include <memory>
#include <Eigen/Dense>
#include "ifkinematicshandle.h"

class IFKinematicsHandle;

static auto ifKinematicsHandle = std::unique_ptr<IFKinematicsHandle>(IFKinematicsHandle::getInstance());


/** 轨迹图元类 */
class PathElement
{

    public:

        ~PathElement();

        PathElement();

        PathElement(Joint begin, Joint end, PathElementType type);

        //PathElement(Point begin, Point end, PathElementType type);

        PathElement(Joint begin, Joint mid, Joint end, PathElementType type);


        /** 获取当前轨迹直线插补后的关节角度数组 */
        std::vector<Joint> getJointArray() const;

        inline void setPathElementType(const PathElementType & pathElementType)
        {
            this->pathElementType = pathElementType;
        }

        inline PathElementType getPathElementType() const
        {
            return pathElementType;
        }

        inline Joint getBeginJoint() const
        {
            return beginJoint;
        }


        inline Joint getEndJoint() const
        {
            return endJoint;
        }

        inline Point getBeginPoint() const
        {
            return beginPoint;
        }

        inline Point getMidPoint() const
        {
            return midPoint;
        }

        inline Point getEndPoint() const
        {
            return endPoint;
        }

    private:

        /** 根据关节角度获取工具尖点坐标 */
        Point Joint2Point(const Joint& joint) const;

        /** 根据工具尖点坐标获取关节角度 */
        Joint Point2Joint(const Point& point) const;

        /** 将位姿矩阵逆解为关节角度 */
        Joint pose_2_Joint(Eigen::Matrix4d poseMatrix) const;

        /** 直线的姿态和位置插补 */
        std::vector<Eigen::Matrix4d> linearPoseAndPositionInterpolation() const;

        /** 圆弧的姿态和位置插补 */
        std::vector<Eigen::Matrix4d> arcPoseAndPositionInterpolation() const;

        /** 根据各关节角度正解获取位姿矩阵 */
        inline Eigen::Matrix4d getPoseMatrix(const Joint& joint) const
        {
            return ifKinematicsHandle->fKine(joint);
        }

        /** 直线的位置插补
         * @p1 起点坐标
         * @p2 终点坐标
         * @t 插补系数
         * @return 根据系数t获取插补中间点坐标
        */
        inline Eigen::Vector3d linearPositionInterpolation(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double t) const
        {
             return  (1 - t) * p1 + t * p2;
        }

        /** 直线的姿态插补
         * @q1 起点姿态的四元数
         * @q2 终点姿态的四元数
         * @t 插补系数
         * @return 根据系数t获取插补中间点的姿态四元数
        */
        inline Eigen::Quaterniond poseInterpolation(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2, double t) const
        {
            return q1.slerp(t, q2);
        }

        /** 位姿矩阵转换为四元数
         * @pose 4*4的姿态矩阵
         * @return 返回位姿矩阵中3*3旋转矩阵(或者叫姿态矩阵)的四元数
        */
        Eigen::Quaterniond poseMatrix_2_quaterniond(const Eigen::Matrix4d& pose) const;

        /** 四元数转换为位姿矩阵
         * @quat 姿态矩阵的四元数
         * @return 返回的是4*4的位姿矩阵, 其中的旋转矩阵(或者叫姿态矩阵)是由四元数转换来的,而位置坐标初始化为(0,0,0)
        */
        Eigen::Matrix4d quaterniond_2_poseMatrix(const Eigen::Quaterniond& quat) const;


        double getAngle(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, Eigen::Vector3d center) const;

        double calculateArcLength(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3) const;

        Eigen::Vector3d arcPositionInterpolation(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, double t) const ;

    private:

        /// 注意机器人运动学句柄是个单例模型, 因此不能作为轨迹element的成员
        /// 因为轨迹elemnt有多个,但是他们的所有句柄指针都指向的是一个单例内存
        /// 当析构轨迹的时候可能会出现多次重复的析构同一个句柄的情况
        //std::unique_ptr<IFKinematicsHandle> ifKinematicsHandle; /// 机器人运动学句柄()

        PathElementType pathElementType;    /// 轨迹类型

        Point beginPoint;     /// 轨迹起点和终点
        Point midPoint;
        Point endPoint;

        Joint beginJoint;     /// 轨迹起点和终点的关节角度
        Joint midJoint;
        Joint endJoint;


};

#endif // PATHELEMENT_H
